#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include "pot_field.h"
#include "main.h"
extern boat myBoat;
VECTOR goal;
VECTOR obs;
VECTOR absd;
void calcAttract(double currLat, double currLon, VECTOR *attForce) {
    double heading;
    double distance;

    distance = lawCosines(currLat, currLon, goal.lat, goal.lon);
    heading = calcHeading(currLat, currLon, goal.lat, goal.lon);
    printf("Goal heading: %lf\n", heading); 
    heading = heading * M_PI / 180;

    if (distance > SPREAD_GOAL) {
        attForce->lat = cos(heading) * MAXSPEED;
        attForce->lon = sin(heading) * MAXSPEED;
    }
    else if (distance > RADIUS_GOAL) {
        attForce->lat = cos(heading) * MAXSPEED / SPREAD_GOAL * distance;
        attForce->lon = sin(heading) * MAXSPEED / SPREAD_GOAL * distance;
    }
    else {
        attForce->lat = 0;
        attForce->lon = 0;
    }

}

void calcRepulse(VECTOR *repForce) {
    double distance;

    distance = sqrt(obs.lat * obs.lat + obs.lon * obs.lon);
    printf("Distance from the obstacle: %lf\n", distance);
 
    if (distance == 0) {
        repForce->lat = 0;
        repForce->lon = 0;
	return;
    }

    if (obs.lon > 0)
    {
        repForce->lon = -obs.lat;
        repForce->lat = obs.lon;
    }
    else
    {
        repForce->lon = obs.lat;
        repForce->lat = -obs.lon;
    }


    double repHeading = myBoat.retHeading * M_PI / 180 + atan(repForce->lon / repForce->lat);
    printf("repHeading: %lf\n", repHeading * 180 / M_PI);    
    if (distance < RADIUS_OBS) {         //use the result above
        repForce->lat = distance * cos(repHeading) * MAXSPEED;
        repForce->lon = distance * sin(repHeading) * MAXSPEED;
    }
    else if (distance < SPREAD_OBS) {   //fine tune the result
        repForce->lat = distance * cos(repHeading) * (1 - distance / SPREAD_OBS) * MAXSPEED;
        repForce->lon = distance * sin(repHeading) * (1 - distance / SPREAD_OBS) * MAXSPEED;
    }
}

void calcComposite(VECTOR *attForce, VECTOR *repForce, double *heading, double *speed) {
    double comLat = attForce->lat + repForce->lat;
    double comLon = attForce->lon + repForce->lon;
    
    *speed = sqrt(comLon * comLon + comLat * comLat);
    if (comLon > 0)
        *heading = acos(comLat / *speed) * 180 / M_PI;
    else
        *heading = -acos(comLat / *speed) * 180 / M_PI;
}

double lawCosines(double lat1, double lon1, double lat2, double lon2)
{
	double radius = 6371000;	
	lat1 = lat1 * M_PI/180;
	lat2 = lat2 * M_PI/180;
	lon1 = lon1 * M_PI/180;
	lon2 = lon2 * M_PI/180;
	double sines = sin(lat1)*sin(lat2);
	double cosines = cos(lat1)*cos(lat2)*cos(lon2-lon1);
	return (acos(sines + cosines) * radius);
}
